//#include "Mahoney.h"
//#include "Madgewick.h"
//#include "vector_math.h"


#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
#define windowSize 3


LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);

//float q[4];
/*
float gAccelX = 0;
float gAccelY = 0;
float gAccelZ = 0;
float gRotX= 0;
float gRotY= 0;
float gRotZ= 0;
*/

float accelX = 0;
float accelY = 0;
float accelZ = 0;
float prevAccelX = 0;
float prevAccelY = 0;
float prevAccelZ = 0;

float rotX= 0;
float rotY= 0;
float rotZ= 0;
float roll= 0;
float pitch= 0;
float yaw= 0;
/*
float compX= 0;
float compY= 0;
float compZ= 0;
*/
//double timeConstant = 2;//makes numbers more managable after integration. Not used currently
float jerkX= 0;//Jerk is the derivative of acceleration
float jerkY= 0;
float jerkZ= 0;
float prevJerkX = 0;
float prevJerkY = 0;
float prevJerkZ = 0;


float accelXBuffer[windowSize];
float accelYBuffer[windowSize];
float accelZBuffer[windowSize];
float rotXBuffer[windowSize];
float rotYBuffer[windowSize];
float rotZBuffer[windowSize];
int iter = 0;

//double accelErrorDecay = 0.5;
//double accelNoiseThreshold = 0.01;


double deltaX;//these three are place holders for calculations
double deltaY;
double deltaZ;
/*
void getRollPitchYaw(){
  //roll = atan2(2 * q0 * q2 - 2 * q0 * q3, 2 * q0*q0 + 2 * q1 * q1 - 1); // psi
  //pitch = -asin(2 * q1 * q3 + 2 * q0 * q2); // theta
  //yaw = atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1); // phi
  
  //roll = atan2(y, sqrt((x * x) + (z * z)));
  //pitch = atan2(x, sqrt((y * y) + (z * z)));
  //yaw = 0;//atan2(-z, sqrt((x*x) + (z * z)));
  
  //pitch *= -180.0 / PI;
  //roll *= -180.0 / PI; 
  //yaw *= -180.0 / PI;
  
  roll = compX;
  pitch = compY;
  yaw = compZ; 
}
*/
/*
void getQuaternion(float e1, float e2, float e3){
  q[0] = sqrt(cos(e2*PI/180)*cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)* cos(e3*PI/180)+1)/2;
  q[1] = (cos(e1*PI/180)*sin(e3*PI/180)+cos(e2*PI/180)*sin(e3*PI/180)+sin(e2*PI/180)*sin(e1*PI/180)*cos(e3*PI/180))/sqrt(cos(e2*PI/180)* cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;
  q[2] = (sin(e2*PI/180)*sin(e3*PI/180)-cos(e2*PI/180)*sin(e1*PI/180)*cos(e3*PI/180)-sin(e1*PI/180))/sqrt(cos(e2*PI/180)*cos(e1*PI/180)+ cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;
  q[3] = (sin(e2*PI/180)*cos(e1*PI/180)+sin(e2*PI/180)*cos(e3*PI/180)+cos(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180))/sqrt(cos(e2*PI/180)* cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;
  
  //q = [cos(a/2), sin(a/2)*roll, sin(a/2)*pitch, sin(a/2)*yaw];
  //MadgwickAHRSupdate(rotX,rotY, rotZ, accelX, accelY, accelZ, compX, compY, compZ);
  //MahonyAHRSupdate(rotX,rotY, rotZ, accelX, accelY, accelZ, compX, compY, compZ);
}
*/
/*
void getGravityVector(){
  dof.readGyro();
  gRotX = dof.calcGyro(dof.gx) * 3.14159 / 100;
  gRotY = dof.calcGyro(dof.gy) * 3.14159 / 100;
  gRotZ = dof.calcGyro(dof.gz) * 3.14159 / 100;
  dof.readAccel();
  gAccelX =  dof.calcAccel(dof.ax);
  gAccelY =  dof.calcAccel(dof.ay);
  gAccelZ =  dof.calcAccel(dof.az);
}
*/


//Quaternion version of removing gravity
/*void removeGravity(){
  accelX = accelX -  (2 * (q[1] * q[3] - q[0] * q[2]));
  accelY = accelY  - (2 * (q[0] * q[1] + q[2] * q[3]));
  accelZ = accelZ -  (q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
}
*/
/*
void printQuaternion(){
  Serial.print(q[0]);Serial.print("\t");
  Serial.print(q[1]);Serial.print("\t");
  Serial.print(q[2]);Serial.print("\t");
  Serial.println(q[3]);
}
*/




//this may not be necessary not that you can set an averaging amount in the analog read.
//Requires testing
/*
void lowPassFilterIMU(){
  accelXBuffer[iter] = accelX;
  accelYBuffer[iter] = accelY;
  accelZBuffer[iter] = accelZ;
  rotXBuffer[iter] = rotX;
  rotYBuffer[iter] = rotY;
  rotZBuffer[iter] = rotZ;
  iter++;
  
  for(int i = 0; i < windowSize; i++){
    if(accelXBuffer[i] != NULL)
    accelX = accelX + accelXBuffer[i];
    if(accelYBuffer[i] != NULL)
    accelY = accelY + accelYBuffer[i];
    if(accelZBuffer[i] != NULL)
    accelZ = accelZ + accelZBuffer[i];
    if(rotXBuffer[i] != NULL)
    rotX = rotX + rotXBuffer[i];
    if(rotYBuffer[i] != NULL)
    rotY = rotY + rotYBuffer[i];
    if(rotZBuffer[i] != NULL)
    rotZ = rotZ + rotZBuffer[i];
  }
  accelX = accelX / windowSize;
  accelY = accelY / windowSize;
  accelZ = accelZ / windowSize;
  rotX = rotX / windowSize;
  rotY = rotY / windowSize;
  rotZ = rotZ / windowSize;
  if(iter == windowSize - 1) iter = 0;
}
*/
void lowPassJerk(){//change this structure to match calibration method
  jerkX = prevJerkX/2 + jerkX/4;
  jerkY = prevJerkY/2 + jerkY/4;
  jerkZ = prevJerkZ/2 + jerkZ/4;
}

void getJerk(){
  //accelX = accelX; //+ deltaAccelX)/timeConstant;//this integrates acceleration
  //deltaAccelY = accelY; //+ deltaAccelY)/timeConstant;//this integrates acceleration
  //deltaAccelZ = accelZ; //+ deltaAccelZ)/timeConstant;//this integrates acceleration    
  //jerkX = accelX;
  //jerkY = accelY;
  //jerkZ = accelZ;
  prevJerkX = jerkX;
  prevJerkY = jerkY;
  prevJerkZ = jerkZ;
  //determine the diference between previous and last velocity reading:
  jerkX = accelX - prevAccelX;
  jerkY = accelY - prevAccelY;
  jerkZ = accelZ - prevAccelZ;
  //record the values from this iteration
  prevAccelX = accelX;
  prevAccelY = accelY;
  prevAccelZ = accelZ;
}
  
void getRaw(){
  //Serial.println("getting rot IMU data");delay(300);
  //Serial.println("entered getting raw");
  dof.readGyro();
  //Serial.println("just read Gyro");
  rotX = dof.calcGyro(dof.gx);
  rotY = dof.calcGyro(dof.gy);
  rotZ = dof.calcGyro(dof.gz);
  //Serial.println("getting accel IMU data");delay(300);
  dof.readAccel();
  accelX =  dof.calcAccel(dof.ax);
  accelY =  dof.calcAccel(dof.ay);
  accelZ =  dof.calcAccel(dof.az);
//ignoring compass for now
  /*dof.readMag();
  compX = dof.calcMag(dof.mx);
  compY = dof.calcMag(dof.my);
  compZ = dof.calcMag(dof.mz);*/
}

void writeIMU_ToData(float* data, float rotSens, float jerkSens){
  //Serial.println("adding to prc");
  data[0] = jerkX * jerkSens;
  data[1] = jerkY * jerkSens;
  data[2] = jerkZ * jerkSens;
  data[3] = rotX  * rotSens;
  data[4] = rotY  * rotSens;
  data[5] = rotZ  * rotSens;
}

void readIMU(float* data, float rotSens, float jerkSens){
  //Serial.println("entered readIMU");
  getRaw();
  //Serial.println("getting raw");
  //getRollPitchYaw();
  //getQuaternion(roll,pitch,yaw);
  //removeGravity();
  //lowPassFilterIMU();   
  getJerk();
  lowPassJerk();
  //Serial.println("read the IMU data, about to write");
  writeIMU_ToData(data, rotSens, jerkSens);
}

